// server.cpp
// #include "websocket_client.h"
// #include "psdk_controller.h"
#include <iostream>
#include <thread>
#include <string>
#include <queue>
#include <mutex>
#include <condition_variable>
#include <chrono>
#include <cstring>
#include "json.hpp"
// #include "dji_gimbal_manager.h"
#include <algorithm>
#include <cctype>
#include <fcntl.h>
#include <sys/stat.h>
#include <unistd.h>
#include <errno.h>
#include <atomic>
#include <sstream>
#include <signal.h>

#include "flight_control_config.h"
#include "status_show_config.h"

// const std::string SERVER_URL = "ws://10.100.68.145:8081/ws/px4";

// 使用 nlohmann::json
using json = nlohmann::json;

// 命令队列和互斥锁
static std::queue<std::string> cmd_queue;
static std::mutex cmd_mutex;
static std::condition_variable cmd_cv;
static bool should_exit = false;

// ROS(管道)控制启用状态标志
static std::atomic<bool> g_is_ros_control_enabled(false);

// // 命名管道的路径
// const char* PIPE_READ_PATH = "/tmp/psdk_pipe_read";
// const char* PIPE_WRITE_PATH = "/tmp/psdk_pipe_write";
// static int pipe_read_fd = -1;
// static int pipe_write_fd = -1;

// 队列最大长度
static const size_t MAX_QUEUE_SIZE = 100;

// 用于等待飞行器静止的常量和函数
static const float VELOCITY_THRESHOLD = 0.1f; // 认为飞行器静止的速度阈值 (m/s)
static const int CHECK_INTERVAL_MS = 100;     // 检查速度的时间间隔 (ms)
static const int MAX_CHECK_TIME_MS = 30000;   // 最大等待时间 (ms)

// // 全局变量，用于存储原点坐标
// static std::atomic<double> g_origin_lat(0.0);
// static std::atomic<double> g_origin_lon(0.0);
// static std::atomic<bool> g_origin_is_set(false);

// static std::atomic<double> g_target_x(0.0);
// static std::atomic<double> g_target_y(0.0);
// static std::atomic<double> g_target_z(0.0);

static bool wait_until_aircraft_stationary() {
    auto start_time = std::chrono::steady_clock::now();
    
    while (true) {
        float vx = 0, vy = 0, vz = 0;
        // psdk_get_velocity(&vx, &vy, &vz);
        
        // 检查三个方向的速度是否都小于阈值
        if (std::abs(vx) < VELOCITY_THRESHOLD && 
            std::abs(vy) < VELOCITY_THRESHOLD && 
            std::abs(vz) < VELOCITY_THRESHOLD) {
            std::cout << "[状态检查] 飞行器已静止。" << std::endl;
            return true;
        }
        
        // 检查是否超时
        auto current_time = std::chrono::steady_clock::now();
        auto elapsed = std::chrono::duration_cast<std::chrono::milliseconds>(
            current_time - start_time).count();
        if (elapsed >= MAX_CHECK_TIME_MS) {
            std::cout << "[状态检查] 等待飞行器静止超时。" << std::endl;
            return false;
        }
        
        std::this_thread::sleep_for(std::chrono::milliseconds(CHECK_INTERVAL_MS));
    }
}

// 处理WebSocket消息的回调函数 
static void handle_websocket_message(const char* message, int len) {
    std::string cmd(message, len);
    try {
        json cmd_json = json::parse(cmd);
        std::string cmd_type = cmd_json.value("command", "");
        
        // 检查是否是高频控制命令
        bool is_realtime = (cmd_type == "vstick_advance_v" || 
                           cmd_type == "vstick_advance_p" || 
                           cmd_type == "set_ros_x_y_z" || 
                           cmd_type == "position_control");
        
        {
            std::lock_guard<std::mutex> lock(cmd_mutex);
            
            // 队列长度检查
            if (cmd_queue.size() >= MAX_QUEUE_SIZE) {
                // 丢弃最旧命令
                cmd_queue.pop();
            }
            
            // 对于高频控制命令，移除队列中同类型的旧命令
            if (is_realtime) {
                std::queue<std::string> new_queue;
                while (!cmd_queue.empty()) {
                    std::string front_cmd = cmd_queue.front();
                    cmd_queue.pop();
                    
                    bool keep_command = true;
                    try {
                        json front_json = json::parse(front_cmd);
                        std::string front_type = front_json.value("command", "");
                        if (front_type == cmd_type) {
                            keep_command = false; // 移除同类型旧命令
                        }
                    } catch (...) {
                        // 非JSON命令保留
                    }
                    
                    if (keep_command) {
                        new_queue.push(front_cmd);
                    }
                }
                cmd_queue = new_queue;
            }
            
            cmd_queue.push(cmd);
        }
        cmd_cv.notify_one();
    } catch (json::parse_error& e) {
        // 非JSON命令
        {
            std::lock_guard<std::mutex> lock(cmd_mutex);
            if (cmd_queue.size() >= MAX_QUEUE_SIZE) {
                cmd_queue.pop();
            }
            cmd_queue.push(cmd);
        }
        cmd_cv.notify_one();
    }
}


extern "C" {
void DjiUser_RunWebServerSample(void) {
    // ros句柄
    ros::NodeHandle nh;
    ros::Rate rate(50.0);
    // while(!ros::ok()){

    // }
    std::string SERVER_URL;
    nh.param<std::string>("SERVER_URL", SERVER_URL, "ws://10.100.68.145:8081/ws/px4");
    
    ros::ServiceClient flight_status_client = nh.serviceClient<px4_flight::flight_status_control>(
        "/flight_status");
    ros::ServiceClient flight_target_client = nh.serviceClient<px4_flight::flight_target_control>(
        "/flight_target");

    std::cout << "正在启动WebSocket客户端，连接到: " << SERVER_URL << std::endl;
    std::thread client_thread(websocket_client_start, SERVER_URL, handle_websocket_message);

    // 启动数据上报线程
    std::thread reporting_thread(px4_status_show);

    // 启动飞行控制线程
    std::thread controlling_thread(px4_flight_control);
    
    // 主线程处理命令
    while (ros::ok() && !should_exit) {
        std::string cmd_str;
        {
            std::unique_lock<std::mutex> lock(cmd_mutex);
            // 使用带超时的等待避免CPU空转
            if (!cmd_cv.wait_for(lock, std::chrono::milliseconds(100), 
                [] { return !cmd_queue.empty() || should_exit; })) {
                continue; // 超时继续循环
            }
            
            if (should_exit) break;
            
            if (!cmd_queue.empty()) {
                cmd_str = cmd_queue.front();
                cmd_queue.pop();
            }
        }
        
        if (!cmd_str.empty()) {
            bool result = false;
            // 优先尝试解析为 JSON (用于虚拟摇杆)
            try {
                json cmd_json = json::parse(cmd_str);
                std::cout << cmd_json["command"] << std::endl;
                if (cmd_json.contains("command") && cmd_json["command"] == "vstick") {
                    if (cmd_json.contains("data")) {
                        auto data = cmd_json["data"];
                        float right_x = data.value("left_stick_x", 0.0f);
                        float right_y = data.value("left_stick_y", 0.0f);
                        float left_x = data.value("right_stick_x", 0.0f);
                        float left_y = data.value("right_stick_y", 0.0f);

                        // PSDK摇杆控制 (安卓的左右手柄与PSDK定义可能相反，这里按常规Mode 2映射)
                        // left_stick (x:yaw, y:throttle), right_stick (x:roll, y:pitch)
                        // psdk_move_by_joystick(yaw, throttle, roll, pitch)
                        // 假设安卓端left_stick_x/y对应yaw/throttle, right_stick_x/y对应roll/pitch
                        // result = psdk_move_by_joystick(left_x, left_y, right_x, right_y);
                        // 摇杆指令通常不打印成功/失败，以避免刷屏
                        px4_flight::flight_target_control target_status;
                        target_status.request.mode = "velocity";
                        target_status.request.yaw = -left_x;
                        target_status.request.z_vp = left_y;
                        target_status.request.x_vp = right_y;
                        target_status.request.y_vp = -right_x;
                        target_status.request.statu = 8; // VSTICK
                        flight_target_client.call(target_status);
                        // 等待响应
                        flight_target_client.waitForExistence();
                        continue; // 处理完摇杆指令后继续循环
                    }
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "vstick_advance_v") {
                    if (cmd_json.contains("data")) {
                        auto data = cmd_json["data"];
                        float yaw = data.value("left_stick_x", 0.0f); // yaw
                        float z = data.value("left_stick_y", 0.0f); // z
                        float y = data.value("right_stick_x", 0.0f); // y
                        float x = data.value("right_stick_y", 0.0f); // x

                        // PSDK摇杆控制 (安卓的左右手柄与PSDK定义可能相反，这里按常规Mode 2映射)
                        //yaw z y x
                        // result = psdk_move_by_joystick_v(left_x, left_y, right_x, right_y);
                        // 摇杆指令通常不打印成功/失败，以避免刷屏
                        px4_flight::flight_target_control target_status;
                        target_status.request.mode = "velocity";
                        target_status.request.yaw = yaw;
                        target_status.request.z_vp = z;
                        target_status.request.x_vp = x;
                        target_status.request.y_vp = y;
                        target_status.request.statu = 8; // VSTICK
                        flight_target_client.call(target_status);
                        // 等待响应
                        flight_target_client.waitForExistence();
                        continue; // 处理完摇杆指令后继续循环
                    }
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "vstick_advance_p") {
                    if (cmd_json.contains("data")) {
                        auto data = cmd_json["data"];
                        float x = data.value("left_stick_x", 0.0f); // x
                        float y = data.value("left_stick_y", 0.0f); // y
                        float yaw = data.value("right_stick_x", 0.0f); // yaw
                        float z = data.value("right_stick_y", 0.0f); // z
                    	// result = psdk_move_by_position_offset(x, y, z, yaw);
                        px4_flight::flight_target_control target_status;
                        target_status.request.mode = "position";
                        target_status.request.yaw = yaw;
                        target_status.request.z_vp = z;
                        target_status.request.x_vp = x;
                        target_status.request.y_vp = y;
                        target_status.request.statu = 8; // VSTICK
                        flight_target_client.call(target_status);
                        // 等待响应
                        flight_target_client.waitForExistence();
                        continue; // 处理完摇杆指令后继续循环
                    }
                }
                if ((cmd_json.contains("command") && cmd_json["command"] == "position_control") ||
                         (cmd_json.contains("type") && cmd_json["type"] == "position_control")) {
                    float x = cmd_json.value("x", 0.0f);
                    float y = cmd_json.value("y", 0.0f);
                    float z = cmd_json.value("z", 0.0f);
                    float yaw = cmd_json.value("yaw", 0.0f);

                    // std::cout << "执行位置控制命令: x=" << x << ", y=" << y << ", z=" << z << ", yaw=" << yaw << std::endl;
                    // result = psdk_move_by_position_offset(x, y, z, yaw);
                    // std::cout << "位置控制命令执行" << (result ? "成功" : "失败") << std::endl;
                    px4_flight::flight_status_control flight_status;
                    flight_status.request.statu = 9;
                    flight_status_client.call(flight_status);
                    // 等待响应
                    flight_status_client.waitForExistence();

                    px4_flight::flight_target_control target_status;
                    target_status.request.mode = "position_control";
                    target_status.request.yaw = yaw;
                    target_status.request.z_vp = z;
                    target_status.request.x_vp = x;
                    target_status.request.y_vp = y;
                    target_status.request.statu = 9; // POSITION
                    flight_target_client.call(target_status);
                    // 等待响应
                    flight_target_client.waitForExistence();
                    continue;
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "set_ros_target")  {
                    float x = cmd_json.value("x", 0.0f);
                    float y = cmd_json.value("y", 0.0f);
                    float z = cmd_json.value("z", 0.0f);
                    g_target_x = x;
                    g_target_y = y;
                    g_target_z = z;
                    std::cout << "set ros: x=" << x << ", y=" << y << ", z=" << z << std::endl;
                    continue;
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "set_ros_target_latlon")  {
                    float lat = cmd_json.value("lat", 0.0f);
                    float lon = cmd_json.value("lon", 0.0f);
                    float height = cmd_json.value("height", 0.0f);
                    float target_x_cal = 0, target_y_cal = 0;
                    // latlon_to_enu(g_origin_lat, g_origin_lon, lat, lon,&target_x_cal,&target_y_cal);
                    g_target_x = target_x_cal;
                    g_target_y = -target_y_cal;
                    g_target_z = 0;
                    std::cout << "set ros: x=" <<target_x_cal << ", y=" << target_y_cal << ", z=" << g_target_z << std::endl;
                    continue;
                }
                if (cmd_json.contains("command") && cmd_json["command"] == "set_ros_origin")  {
                    float lat = cmd_json.value("lat", 0.0f);
                    float lon = cmd_json.value("lon", 0.0f);
                    g_origin_lat = lat;
                    g_origin_lon = lon;
                    g_origin_is_set = true;
                    continue;
                }
            } catch (json::parse_error& e) {
                // 解析失败，说明是简单文本命令，继续往下执行
            }
         // 处理RTMP直播命令
            if (cmd_str.find("start_rtmp_live ") == 0) {
                // 18 is the length of "start_rtmp_live "
                std::string url = cmd_str.substr(16); 
                std::cout << "执行开启RTMP直播命令, URL: " << url << std::endl;
                // result = psdk_start_rtmp_live(url.c_str());
                std::cout << "开启RTMP直播命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "stop_rtmp_live") {
                std::cout << "执行停止RTMP直播命令" << std::endl;
                // result = psdk_stop_rtmp_live();
                std::cout << "停止RTMP直播命令执行" << (result ? "成功" : "失败") << std::endl;
            // 处理简单文本命令 (与安卓端命令对齐)
	        } else if (cmd_str == "takeoff") {
                px4_flight::flight_status_control flight_status;
                flight_status.request.statu = 1;
                flight_status_client.call(flight_status);
                // 等待响应
                flight_status_client.waitForExistence();
            } else if (cmd_str == "land") {
                px4_flight::flight_status_control flight_status;
                flight_status.request.statu = 5;
                flight_status_client.call(flight_status);
                // 等待响应
                flight_status_client.waitForExistence();
            } else if (cmd_str == "confirm_land") {
                std::cout << "执行queren降落命令" << std::endl;
                // result = psdk_confirm_land();
                std::cout << "降落命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "go_home") {
                std::cout << "执行返航命令" << std::endl;
                // result = psdk_go_home();
                std::cout << "返航命令执行" << (result ? "成功" : "失败") << std::endl;
            } else if (cmd_str == "cancel_go_home") {
                // 本来是大疆的取消返航，现在用作悬停
                px4_flight::flight_status_control flight_status;
                flight_status.request.statu = 4;
                flight_status_client.call(flight_status);
                // 等待响应
                flight_status_client.waitForExistence();
            } else if (cmd_str == "enable_vstick") { // PSDK中是获取控制权
                // std::cout << "获取摇杆控制权限" << std::endl;
                // result = psdk_obtain_joystick_control();
                // std::cout << "获取摇杆控制权限" << (result ? "成功" : "失败") << std::endl;
                px4_flight::flight_status_control flight_status;
                flight_status.request.statu = 8;
                flight_status_client.call(flight_status);
                // 等待响应
                flight_status_client.waitForExistence();
            } else if (cmd_str == "disable_vstick") { // PSDK中是释放控制权
                // std::cout << "释放摇杆控制权限" << std::endl;
                // result = psdk_release_joystick_control();
                // std::cout << "释放摇杆控制权限" << (result ? "成功" : "失败") << std::endl;
                px4_flight::flight_status_control flight_status;
                flight_status.request.statu = 4;
                flight_status_client.call(flight_status);
                // 等待响应
                flight_status_client.waitForExistence();
            } else if (cmd_str == "enable_ros_control") { // 允许or取消算法控制无人机飞行
                // std::cout << "释放摇杆控制权限" << std::endl;
                // result = psdk_release_joystick_control();
                // std::cout << "释放摇杆控制权限" << (result ? "成功" : "失败") << std::endl;
                px4_flight::flight_status_control flight_status;
                if (phase != ALGORITHM) flight_status.request.statu = 10;
                else if (phase == ALGORITHM) flight_status.request.statu = 4;
                flight_status_client.call(flight_status);
                // 等待响应
                flight_status_client.waitForExistence();
            }
            // } else if (cmd_str == "psdk_gimbal_reset") {
            //     std::cout << "执行云台复位命令" << std::endl;
            //     // result = DjiGimbalManager_Init();
            //     // if (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            //     //     result = DjiGimbalManager_Reset(E_DjiMountPosition(1), DJI_GIMBAL_RESET_MODE_PITCH_AND_YAW);
            //     //     DjiGimbalManager_Deinit();
            //     // }
            //     // std::cout << "云台复位命令执行" << (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS ? "成功" : "失败") << std::endl;
            // } else if (cmd_str.find("psdk_gimbal_mode:") == 0) {
            //     // 注意: "psdk_gimbal_mode:" 是 19 个字符
            //     std::string mode = cmd_str.substr(19); 
            //     // E_DjiGimbalMode gimbalMode = DJI_GIMBAL_MODE_FREE;
                
            //     // if (mode == "yaw_follow") {
            //     //     gimbalMode = DJI_GIMBAL_MODE_YAW_FOLLOW;
            //     // }
                
            //     std::cout << "设置云台模式: " << mode << std::endl;
            //     // result = DjiGimbalManager_Init();
            //     // if (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            //     //     result = DjiGimbalManager_SetMode(E_DjiMountPosition(1), gimbalMode);
            //     //     DjiGimbalManager_Deinit();
            //     // }
            //     // std::cout << "设置云台模式" << (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS ? "成功" : "失败") << std::endl;
            // } else if (cmd_str.find("psdk_gimbal_rotate:") == 0) {
            //     // 注意: "psdk_gimbal_rotate:" 是 21 个字符
            //     std::string values = cmd_str.substr(21); 
            //     float pitch = 0, yaw = 0;
                
            //     try {
            //         // 移除所有空格和不可见字符
            //         values.erase(std::remove_if(values.begin(), values.end(), 
            //             [](unsigned char c) { return std::isspace(c) || !std::isprint(c); }), 
            //             values.end());
                    
            //         size_t pos = values.find(",");
            //         if (pos != std::string::npos) {
            //             pitch = std::stof(values.substr(0, pos));
            //             yaw = std::stof(values.substr(pos + 1));
                        
            //             std::cout << "控制云台: pitch=" << pitch << ", yaw=" << yaw << std::endl;
                        
            //             // result = DjiGimbalManager_Init();
            //             // if (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            //             //     T_DjiGimbalManagerRotation rotation = {
            //             //         .rotationMode = DJI_GIMBAL_ROTATION_MODE_RELATIVE_ANGLE,
            //             //         .pitch = pitch,
            //             //         .roll = 0,
            //             //         .yaw = yaw,
            //             //         .time = 0.5f
            //             //     };    
                            
            //             //     result = DjiGimbalManager_Rotate(E_DjiMountPosition(1), rotation);
            //             //     if (result != DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS) {
            //             //         std::cout << "云台旋转失败，错误码: 0x" << std::hex << result << std::endl;
            //             //     }
            //             //     DjiGimbalManager_Deinit();
            //             // } else {
            //             //     std::cout << "云台初始化失败，错误码: 0x" << std::hex << result << std::endl;
            //             // }
            //         } else {
            //             std::cout << "无效的云台控制数据格式: " << values << std::endl;
            //             result = false;
            //         }
            //     } 
            //     catch (const std::exception& e) {
            //         std::cerr << "解析云台控制数据失败: " << e.what() << std::endl;
            //         std::cerr << "原始数据: " << values << std::endl;
            //         result = false;
            //     }
            //     // std::cout << "云台控制命令执行" << (result == DJI_ERROR_SYSTEM_MODULE_CODE_SUCCESS ? "成功" : "失败") << std::endl;
		    // }
		else {
                // std::cout << "未知或非摇杆命令: " << cmd_str << std::endl;
            }
        }
        ros::spinOnce();
        rate.sleep();
    }

    // 等待线程结束
    if (client_thread.joinable()) client_thread.join();
    if (reporting_thread.joinable()) reporting_thread.join();
    if (controlling_thread.joinable()) controlling_thread.join();
    
    // 清理命名管道
    // close(pipe_read_fd);
    // close(pipe_write_fd);
    // unlink(PIPE_READ_PATH);
    // unlink(PIPE_WRITE_PATH);
    
    std::cout << "客户端已关闭" << std::endl;
}

void trigger_server_exit() {
    {
        std::lock_guard<std::mutex> lock(cmd_mutex);
        should_exit = true;
    }
    cmd_cv.notify_all(); // 唤醒所有等待的线程
    websocket_client_stop();
}

void sigint_handler(int sig) {
    ros::shutdown();
    trigger_server_exit();
}
}

int main(int argc, char **argv) {
    // ROS节点初始化
    ros::init(argc, argv, "px4_flight_control");
    signal(SIGINT, sigint_handler);  // 捕获 Ctrl+C
    DjiUser_RunWebServerSample();

    return 0;
}